#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include "urobot.h"
#include "target.h"
#include "services.h"
#include "sms.h"

#include "linuxuvc.h"

#define ADCID		0x00AD
#define ACCID		0x00AC
#define CAMID		0x0C

pthread_t sensor_thread_id;
void *sensor_thread(void *arg);

/* Car Status Check */
int go = 0;
int back = 0;
int left = 0;
int right = 0;
int fog = 0;
int front = 0;
int emer = 0;
int dm_right = 0;
int dm_left = 0;
int breakl = 0;

int camera_id = 0;
int camera_type = 0;
char camera_encoding[] = "multipart-jpeg";

/* globals */
static globals global;

int init_camera(void);
int init_sensor(void);

int get_adc_data(unsigned short *send_data)
{
	int i;

	for(i=1; i<5; i++)
		send_data[i] = (unsigned short)(1000*(rand()/(RAND_MAX+1.0)));
	send_data[0] = ADCID;
			
	return 0;
}

int get_acc_data(unsigned short *send_data)
{
	int i;

	for(i=1;i<4;i++)
		send_data[i] = (unsigned short)(1000*(rand()/(RAND_MAX+1.0)));
	send_data[4] = 0x00; //dummy
	send_data[0] = ACCID;

	return 0;
}

int get_sensor_data(int type, unsigned short *send_data)
{
	if (type == LIGHT_SENSOR) {
		send_data[0] = front;
		send_data[1] = fog;
		send_data[2] = breakl;
		send_data[3] = (emer << 2) + (right << 1) + left;
		return 0;
	}
	else if (type == ACCELERATOR_SENSOR)
		return get_acc_data(send_data);

	else if (type == DISTANCE_SENSOR)
		return get_adc_data(send_data);

	else
		return -1;
}

static int sensor_thread_run = 0;
static char values[7][20];
static char *pvalues[7] = {values[0], values[1], values[2], values[3], values[4], values[5], values[6]};
void *sensor_thread(void *arg)
{
	int sleeptime = 2;
	unsigned short send_data[5];
	char value[20];

	while (sensor_thread_run) {
		sleep((unsigned int)sleeptime);

		get_adc_data(send_data);
		/*
		sprintf(value, "%d", send_data[1]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_FRONT_DISTANCE, value);
		sprintf(value, "%d", send_data[2]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_BACK_DISTANCE, value);
		sprintf(value, "%d", send_data[3]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_LEFT_DISTANCE, value);
		sprintf(value, "%d", send_data[4]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_RIGHT_DISTANCE, value);
		*/
		sprintf(values[0], "%d", send_data[1]);
		sprintf(values[1], "%d", send_data[2]);
		sprintf(values[2], "%d", send_data[3]);
		sprintf(values[3], "%d", send_data[4]);

		get_acc_data(send_data);
		/*
		sprintf(value, "%d", send_data[1]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_ACCELERATOR_X, value);
		sprintf(value, "%d", send_data[2]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_ACCELERATOR_Y, value);
		sprintf(value, "%d", send_data[3]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_ACCELERATOR_Z, value);
		*/
		sprintf(values[4], "%d", send_data[1]);
		sprintf(values[5], "%d", send_data[2]);
		sprintf(values[6], "%d", send_data[3]);
		printf("values=%s\n", *values);
		upnp_set_variables(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_FRONT_DISTANCE, pvalues, 7);
		
	}

	return NULL;
}

int set_sensor_data(int type, int index, int value)
{
	char cmd_buf;
	int result = 0;

	if (type != LIGHT_SENSOR)
		return -1;

	switch(index){
	case FRONT_LIGHT_SENSOR:
		front = (front == 0) ? 1 : 0;
		printf("Front light sets to be %s\n", front ? "ON":"OFF");
		result = front;
		break;
	case FOG_LIGHT_SENSOR:
		fog = (fog == 0) ? 1 : 0;
		printf("Fog light sets to be %s\n", fog ? "ON":"OFF");
		result = fog;
		break;
	case BREAK_LIGHT_SENSOR:
		breakl = (breakl == 0) ? 1 : 0;
		printf("Break light sets to be %s\n", breakl ? "ON":"OFF");
		result = breakl;
		break;
	case WINKER_LIGHT_SENSOR:
		if (value == 0) {
			emer = (emer == 0) ? 1 : 0;
			printf("Emergency light sets to be %s\n", emer ? "ON":"OFF");
			result = emer;
		}
		else if (value == 1) {
			left = (left == 0) ? 1 : 0;
			printf("Leftturn light sets to be %s\n", emer ? "ON":"OFF");
			result = left;
		}
		else if (value == 2) {
			right = (right == 0) ? 1 : 0;
			printf("Rightturn light sets to be %s\n", emer ? "ON":"OFF");
			result = right;
		}
		break;
	}
	return result;
}

int set_motion(int cmd, int value)
{
	char cmd_buf;
	int result = 0;

	switch(cmd){
	case FORWARD_MOTION:
		if (go == 0) {
			usleep(10000);
			go = 1;
			back = 0;
			printf("GoForward motion was set\n");
		}
		else {
			go = 0;
			back = 0;
			printf("GoForward motion was not set\n");
		}
		result = go;
		break;
	case RIGHTTURN_MOTION:
		if(dm_right == 0){
			dm_right = 1;
			dm_left = 0;
			printf("TurnRight motion was set\n");
		}
		else{
			dm_right = 0;
			printf("TurnRight motion was not set\n");
		}
		result = dm_right;
		break;
	case BACKWARD_MOTION:
		if(back == 0){
			usleep(10000);
			back = 1;
			go = 0;
			printf("GoBackward motion was set\n");
		}
		else{
			back = 0;
			go = 0;
			printf("GoBackward motion was not set\n");
		}
		result = back;
		break;
	case LEFTTURN_MOTION:
		if(dm_left == 0){
			dm_right = 0;
			dm_left = 1;
			printf("TurnLeft motion was set\n");
		}
		else{
			dm_left = 0;
			printf("TurnLeft motion was not set\n");
		}
		result = dm_left;
		break;
	case STOP_MOTION:
		back = 0;
		go = 0;
		dm_right = 0;
		dm_left = 0;
		break;
	}
	return result;
}

int get_camera_data(int id, int *type, char *encoding)
{
	if (id != camera_id)
		return -1;

	*type = camera_type;  /* 0:stream, 1:still */

	strcpy(encoding, camera_encoding); /* motion-jpeg: "mjpg", "mjpeg" */

	return 0;
}

int set_camera_data(int id, int type, char *encoding)
{
	if (id != 0)
		return -1;

	if (type == 0 || type == 1) /* 0:stream,  1:still */
		camera_type = type;

	strcpy(camera_encoding, encoding); /* motion-jpeg: "mjpg", "mjpeg" */

	return 0;
}

void init_device(int argc, char *argv[])
{
	start_camera_thread();
	return 0;
}

void finish_device(int argc, char *argv[])
{
	sensor_thread_run = 0;

	global.stop = 1;
	usleep(1000 * 1000);

	global.in[0].stop(0);
	global.out[0].stop(global.out[0].param.id);
	pthread_cond_destroy(&global.in[0].db_update);
	pthread_mutex_destroy(&global.in[0].db);

	return;
}

int start_sensor_thread()
{
	if (sensor_thread_run) {
		printf("Sensor thread is already running!\n");
		return -1;
	}
	printf("Thread Create\n");
	sensor_thread_run = 1;
	if(pthread_create(&sensor_thread_id, NULL, sensor_thread, NULL) < 0) {
		printf("sensor thread create error\n");
		return -1;
	}

	return 0;
}

int split_parameters(char *parameter_string, int *argc, char **argv)
{
    int count = 1;
    argv[0] = NULL; // the plugin may set it to 'INPUT_PLUGIN_NAME'
    if(parameter_string != NULL && strlen(parameter_string) != 0) {
        char *arg = NULL, *saveptr = NULL, *token = NULL;

        arg = strdup(parameter_string);

        if(strchr(arg, ' ') != NULL) {
            token = strtok_r(arg, " ", &saveptr);
            if(token != NULL) {
                argv[count] = strdup(token);
                count++;
                while((token = strtok_r(NULL, " ", &saveptr)) != NULL) {
                    argv[count] = strdup(token);
                    count++;
                    if(count >= MAX_PLUGIN_ARGUMENTS) {
                        IPRINT("ERROR: too many arguments to input plugin\n");
                        return 0;
                    }
                }
            }
        }
    }
    *argc = count;
    return 1;
}

int start_camera_thread()
{
    char *input[MAX_INPUT_PLUGINS];
    char *output[MAX_OUTPUT_PLUGINS];

    input[0] = "--resolution 640x480 --fps 5 --device /dev/video0";
    global.incnt = 1;

    output[0] = "--port 8080";
    global.outcnt = 1;

    if(pthread_mutex_init(&global.in[0].db, NULL) != 0) {
        LOG("could not initialize mutex variable\n");
        closelog();
        exit(EXIT_FAILURE);
    }
    if(pthread_cond_init(&global.in[0].db_update, NULL) != 0) {
        LOG("could not initialize condition variable\n");
        closelog();
        exit(EXIT_FAILURE);
    }

    global.in[0].param.parameters = strchr(input[0], ' ');
    split_parameters(global.in[0].param.parameters, &global.in[0].param.argc, global.in[0].param.argv);
    global.in[0].param.global = &global;
    global.in[0].param.id = 0;
    if(input_init(&global.in[0].param, 0)) {
        LOG("input_init() return value signals to exit\n");
        closelog();
        exit(0);
    }
    global.out[0].param.parameters = strchr(output[0], ' ');
    split_parameters(global.out[0].param.parameters, &global.out[0].param.argc, global.out[0].param.argv);
    global.out[0].param.global = &global;
    global.out[0].param.id = 0;
    if(output_init(&global.out[0].param, 0)) {
        LOG("output_init() return value signals to exit\n");
        closelog();
        exit(0);
    }
    DBG("starting input plugin\n");
    if(input_run(0)) {
        LOG("can not run input plugin\n");
        closelog();
        return 1;
    }
    DBG("starting output plugin\n");
    output_run(0);

    return 0;
}
